#include<p18f2550.h>
#include<plib/delays.h>

#pragma config FOSC=XT_XT, FCMEN=OFF, IESO=OFF, CPUDIV=OSC1_PLL2
#pragma config PWRT=ON, BOR=OFF, BORV=0, WDT=OFF, WDTPS=32768
#pragma config MCLRE=ON, LPT1OSC=OFF, PBADEN=OFF, CCP2MX=OFF
#pragma config STVREN=OFF, LVP=OFF, XINST=OFF, DEBUG=OFF
#pragma config CP0=OFF, CP1=OFF, CP2=OFF, CPB=OFF, CPD=OFF
#pragma config WRT0=OFF, WRT1=OFF, WRT2=OFF
#pragma config WRTB=OFF, WRTC=OFF, WRTD=OFF
#pragma config EBTR0=OFF, EBTR1=OFF, EBTR2=OFF, EBTRB=OFF


int  Keypad(void){
while(1){
PORTB=1;
if(PORTBbits.RB4){PORTB=0;return 1;}
else if(PORTBbits.RB5){PORTB=0;return 2;}
else if(PORTBbits.RB6){PORTB=0;return 3;}
else if(PORTBbits.RB7){PORTB=0;return 10;}

PORTB=2;
if(PORTBbits.RB4){PORTB=0;return 4;}
else if(PORTBbits.RB5){PORTB=0;return 5;}
else if(PORTBbits.RB6){PORTB=0;return 6;}
else if(PORTBbits.RB7){PORTB=0;return 11;}

PORTB=4;
if(PORTBbits.RB4){PORTB=0;return 7;}
else if(PORTBbits.RB5){PORTB=0;return 8;}
else if(PORTBbits.RB6){PORTB=0;return 9;}
else if(PORTBbits.RB7){PORTB=0;return 12;}

PORTB=8;
if(PORTBbits.RB4){PORTB=0;return 13;}
else if(PORTBbits.RB5){PORTB=0;return 0;}
else if(PORTBbits.RB6){PORTB=0;return 14;}
else if(PORTBbits.RB7){PORTB=0;return 15;}
}
}
int aux=0,a=0,lock=0;;
void main () {

    ADCON1=15;TRISC=0;TRISB=0xF0;TRISA=0;
   

    while(1){
       aux=Keypad();
       if (aux==13){
           a++;
           lock=a/2;if(a<=4){lock=0;a=a;}
       }
       if (lock==0){
        switch(aux)
        {
            case 1:{    // UP Motor
                PORTAbits.RA0=1;
                PORTAbits.RA1=0;
                Delay10KTCYx(50);
                PORTAbits.RA0=0;
                PORTAbits.RA1=0;}
            break;

        case 4:{   //Down Motor
                PORTAbits.RA0=0;
                PORTAbits.RA1=1;
                Delay1KTCYx(50);
                PORTAbits.RA0=0;
                PORTAbits.RA1=0;}
        break;

         case 10:{    // Right Motor
                 PORTAbits.RA2=1;
                PORTAbits.RA3=0;
                Delay1KTCYx(50);
                PORTAbits.RA2=0;
                PORTAbits.RA3=0;}
            break;

        case 3:{   //Left Motor
                PORTAbits.RA3=0;
                PORTAbits.RA3=1;
                Delay1KTCYx(50);
                PORTAbits.RA3=0;
                PORTAbits.RA3=0;}
        break;

         case 2:{    // HandUp Motor
                PORTCbits.RC4=1;
                PORTCbits.RC5=0;
                Delay1KTCYx(50);
                 PORTCbits.RC4=0;
                PORTCbits.RC5=0;}
            break;

        case 5:{   //HandDown Motor
                PORTCbits.RC4=0;
                PORTCbits.RC5=1;
                Delay1KTCYx(50);
                PORTCbits.RC4=0;
                PORTCbits.RC5=0;}
        break;

        case 11:{    // Open Gripper
                 PORTCbits.RC6=1;
                PORTCbits.RC7=0;
                Delay1KTCYx(50);
                 PORTCbits.RC6=0;
                PORTCbits.RC7=0;}
            break;

        case 6:{   //Close Gripper
                PORTCbits.RC6=0;
                PORTCbits.RC7=1;
                Delay1KTCYx(50);
                PORTCbits.RC6=0;
                PORTCbits.RC7=0;}
        break;

        default:
        {
         aux=0;}
        }}}}

